 
#include <vxWorks.h>
#include <vsbConfig.h>
#include <intLib.h>
#include <errnoLib.h>
#include <errno.h>
#include <sioLib.h>
#include <ioLib.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <vxAtomicLib.h>
#include <semLib.h>
#include <sysLib.h>
#include <vmLib.h>
#include <stdio.h>
#include <hwif/vxBus.h>
#include <hwif/buslib/vxbFdtLib.h>
#include <taskLib.h>
#include <pipeDrv.h>      /* 管道驱动 */
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <sys/times.h>  // Add this include for struct timeval
#include "axiSrio.h"
#include "icd.h"


/* SRIO设备函数声明 */
void axiSrioOpen(VXB_DEV_ID *ppDev, UINT32 unit);
int axiSrioClose(VXB_DEV_ID pDev);
STATUS axiSrioIntConnect(VXB_DEV_ID pDev, VOIDFUNCPTR isr);
STATUS axiSrioSend(VXB_DEV_ID pDev, const void *pData, UINT32 dataLen, UINT32 destId);
void *axiSrioGetRxAddr(VXB_DEV_ID pDev);

 


// 全局变量
static VXB_DEV_ID g_srioDev = NULL;
static SEM_ID g_srioSem = NULL;

int pipeFd[2]; /* 管道文件描述符数组: [0]为读端, [1]为写端 */
 

#define PIPE_NAME "/pipe/myEventPipe"
#define PIPE_MSG_SIZE 1
#define PIPE_NUM_MSGS 10


#define BUFFER_SIZE 10240
#define HEADER_SIZE 4  // 假设包头长度为4字节

typedef struct {
    char data[BUFFER_SIZE];
    int length;
    int header_found;
    int data_length;
} PacketBuffer;


static void srio_isr() {
    semGive(g_srioSem);
}   

STATUS pipeInit(void)
{
    // 检查管道是否已存在
    if (pipeFd[0] != 0 || pipeFd[1] != 0) {
        return OK;  // 管道已初始化，直接返回成功
    }

    /* 创建管道设备 */
    if (pipeDevCreate(PIPE_NAME, PIPE_NUM_MSGS, PIPE_MSG_SIZE) != OK) {
        printf("Error creating pipe device\n");
        return ERROR;
    }
    
    /* 打开管道读端 */
    if ((pipeFd[0] = open(PIPE_NAME, O_RDONLY, 0)) == -1) {
        printf("Error opening pipe read end\n");
        return ERROR;
    }
    
    /* 打开管道写端 */
    if ((pipeFd[1] = open(PIPE_NAME, O_WRONLY, 0)) == -1) {
        printf("Error opening pipe write end\n");
        close(pipeFd[0]);
        return ERROR;
    }
    
    return OK;
}
 

 
// 新增：处理客户端数据转发的函数
void client_forward_handler(int client_fd) {
    PacketBuffer packet_buf = {0}; // 初始化缓冲区
    fd_set read_fds;
    int max_fd = client_fd;
    int i;
    static IcdPacket saved_packet;

	   
    /* 确保管道已初始化 */
    if (pipeInit() != OK) {
        return;
    }
    max_fd = (pipeFd[0] > client_fd) ? pipeFd[0] : client_fd;
    max_fd++;  // select需要最大fd+1


    while (1) {
        FD_ZERO(&read_fds);
        FD_SET(client_fd, &read_fds);
        FD_SET(pipeFd[0], &read_fds);

  

        // Add timeout to prevent infinite wait
        struct timeval tv = {1, 0};  
        if (max_fd < 0) {
            printf("Invalid file descriptors\n");
            break;
        }
        // 等待套接字有数据可读
        int activity = select(max_fd + 1, &read_fds, NULL, NULL, &tv);
        if (activity < 0) {
            if (errno != EINTR) {  // Ignore interrupted system calls
                perror("select error");
                break;
            }
            continue;
        }
        if (activity == 0) {
            // Timeout occurred, check if we should continue
            continue;
        }
		// 处理TCP客户端数据
        if (FD_ISSET(client_fd, &read_fds)) {
            int recv_len = recv(client_fd, packet_buf.data + packet_buf.length, 
				BUFFER_SIZE - packet_buf.length, 0);
				if (recv_len <= 0) {
				break;
				}
				packet_buf.length += recv_len;
			// 查找包头
			while (packet_buf.length >= sizeof(IcdPacket)) {
				IcdPacket* packet = (IcdPacket*)packet_buf.data;			
                logMsg("Valid packet header found, processing packet\n", 0, 0, 0, 0, 0, 0);
				// 检查包头标识
				if (packet->header_flag == 0x5AA55AA5) {
					
					// 处理完整数据包
                    // 假设 IcdPacket 结构体中 command_type 字段表示指令类型
                    // 假设需要保存的指令类型为 ICD_DATA_TRANSFER
                    if ((packet->command_type == ICD_DATA_TRANSFER)&&(packet->payload[1] == 0xff)) {
                        memcpy(&saved_packet, packet, sizeof(IcdPacket));
                        memcpy(&saved_packet, packet, sizeof(IcdPacket));
                        // 打印 saved_packet 第6-10个字节
                        logMsg("Saved packet bytes 6-10: 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X\n", 
                               ((unsigned char*)&saved_packet)[5], 
                               ((unsigned char*)&saved_packet)[6], 
                               ((unsigned char*)&saved_packet)[7], 
                               ((unsigned char*)&saved_packet)[8], 
                               ((unsigned char*)&saved_packet)[9], 0);
                        
                    }
                    else if ((packet->command_type == ICD_DATA_TRANSFER)&&(packet->payload[1] == 0x00)) {
                        // 发送保存的数据包
                        STATUS status = axiSrioSend(g_srioDev, &saved_packet, sizeof(IcdPacket), 0);
                        memset  (&saved_packet, 0, sizeof(IcdPacket));  
                    }

                            

                    STATUS status = axiSrioSend(g_srioDev, packet, 1024, 0);
                    if (status != OK) {
                        logMsg("SRIO send failed\n", 0, 0, 0, 0, 0, 0);
                    }

 
					memmove(packet_buf.data, packet_buf.data + 1024, 
						packet_buf.length - 1024);
					packet_buf.length -= 1024;	 
					continue;  // 继续处理缓冲区中可能存在的下一个包
				}
				break;  // 数据不完整，等待更多数据
		 
				
				// 包头无效，滑动一个字节继续查找
				memmove(packet_buf.data, packet_buf.data + 1, packet_buf.length - 1);
				packet_buf.length--;
			}

  
        }

       // 处理SRIO事件
        if (FD_ISSET(pipeFd[0], &read_fds)) {
             char event;
            if (read(pipeFd[0], &event, sizeof(event)) == sizeof(event)) {
                    // 读取SRIO接收缓冲区
                    void* rxAddr = axiSrioGetRxAddr(g_srioDev);  
                    int flag = 0;
                    if (rxAddr != NULL) {
                        IcdPacket* packet = (IcdPacket*)rxAddr;           
                                     
//                        logMsg("Valid packet received with command type: %d\n", packet->command_type, 0, 0, 0, 0, 0);
                        // 验证包头
                        if (packet->header_flag != 0x5AA55AA5) {
                            continue;
                        }// 处理命令
                        switch (packet->command_type) {
                            case ICD_HANDSHAKE:
                                logMsg("Processing SLAVE ICD_HANDSHAKE response\n", 0, 0, 0, 0, 0, 0);
                                break;
                            case ICD_MODE_SET:
                                logMsg("Processing SLAVE ICD_MODE_SET response\n", 0, 0, 0, 0, 0, 0);
                                 break;
                            case ICD_ACQ_CTRL:
                                logMsg("Processing SLAVE ICD_ACQ_CTRL response\n", 0, 0, 0, 0, 0, 0);
                                 break;
                            case ICD_DATA_TRANSFER_RIO:
//                                logMsg("Processing SLAVE ICD_DATA_TRANSFER response\n", 0, 0, 0, 0, 0, 0);
                                packet->command_type = ICD_DATA_TRANSFER;
                                flag = 1;
                                break;
                            case ICD_DATA_TRANSFER_RIOEND:
                                logMsg("Processing SLAVE ICD_DATA_TRANSFER response\n", 0, 0, 0, 0, 0, 0);
                                packet->command_type = ICD_DATA_TRANSFER;
                                flag = 0;
                                break;
                            default:
                                logMsg("Unknown SRIO command: %d\n", packet->command_type, 0, 0, 0, 0, 0);
                                continue;
                        }
 
                        char client_ip[INET_ADDRSTRLEN];
                        struct sockaddr_in client_addr;
                        socklen_t client_addr_len = sizeof(client_addr);
                        if (getpeername(client_fd, (struct sockaddr *)&client_addr, &client_addr_len) == 0) {
                            inet_ntop(AF_INET, &client_addr.sin_addr, client_ip, INET_ADDRSTRLEN);
                        } else {
                            logMsg("Failed to get client address for fd %d\n", client_fd, 0, 0, 0, 0, 0);
                        }
                        send(client_fd, rxAddr, sizeof(IcdPacket), 0);
                        if(flag)
                        {
                        	saved_packet.payload[1] = 0xfe;
                            axiSrioSend(g_srioDev, &saved_packet, 1024, 0);
                        }
                    }
                
            }
        }	
    }
 
    close(client_fd);
}


// 新增：转发服务器启动函数
STATUS tcp_forward_server_start(unsigned short port) {
    int server_fd;
    struct sockaddr_in server_addr;

    server_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (server_fd < 0) {
        perror("创建转发服务器 socket 失败");
        return ERROR;
    }
    printf("TCP转发服务器已启动，监听端口: %d (0x%X)\n", port, port);  // 同时打印十进制和十六进制端口号

    memset(&server_addr, 0, sizeof(server_addr));
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(port);
    server_addr.sin_addr.s_addr = INADDR_ANY;

    if (bind(server_fd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
        perror("绑定转发服务器端口失败");
        close(server_fd);
        return ERROR;
    }

    if (listen(server_fd, 5) < 0) {
        perror("转发服务器监听失败");
        close(server_fd);
        return ERROR;
    }

    printf("转发TCP服务器已启动，监听端口: %d\n", port);

    while (1) {
        struct sockaddr_in client_addr;
        socklen_t client_addr_len = sizeof(client_addr);
        int client_fd = accept(server_fd, (struct sockaddr *)&client_addr, &client_addr_len);
        if (client_fd < 0) {
            perror("接受客户端连接失败");
            continue;
        }
        char client_ip[INET_ADDRSTRLEN];
        inet_ntop(AF_INET, &client_addr.sin_addr, client_ip, INET_ADDRSTRLEN);
        printf("客户端连接成功，IP: %s，端口: %d\n", client_ip, ntohs(client_addr.sin_port));

        // 创建新任务处理客户端连接
        if (taskSpawn("tcp_forward_client", 100, VX_FP_TASK, 819200, (FUNCPTR)client_forward_handler, 
                     client_fd, 0, 0, 0, 0, 0, 0, 0, 0, 0) == ERROR) {
            perror("创建客户端处理任务失败");
            close(client_fd);  // 任务创建失败，关闭客户端连接
        }
    }

    return OK;
}
 

  
static void srio_receive_task(void) {
    char eventMsg = 'E'; /* 事件通知消息 */

    while (1) {
        if (semTake(g_srioSem, WAIT_FOREVER) == OK) {
            if (write(pipeFd[1], &eventMsg, sizeof(eventMsg)) != sizeof(eventMsg)) {
                        printf("Error writing to pipe\n");
                    }
        }
    }
}
// 初始化SRIO客户端
STATUS srio_client_init() {


    g_srioSem = semBCreate(SEM_Q_PRIORITY, SEM_EMPTY);
    if (g_srioSem == NULL) {
        return ERROR;
    }
    axiSrioOpen(&g_srioDev,0);

    axiSrioIntConnect(g_srioDev, (VOIDFUNCPTR)srio_isr);

    
    

    // 以任务的方式启动 TCP 转发服务器
    // 任务优先级设为 100，栈大小设为 4096，可根据实际情况调整
    if (taskSpawn("tTcpForwardServer", 100, 0, 4096, (FUNCPTR)tcp_forward_server_start, 8800, 0, 0, 0, 0, 0, 0, 0, 0, 0) == ERROR) {
        printf("Failed to spawn tcp forward server task\n");
    }
        // 创建 SRIO 接收任务
        // 任务优先级设为 100，栈大小设为 4096，可根据实际情况调整
    if (taskSpawn("tSrioReceive", 100, 0, 4096, (FUNCPTR)srio_receive_task, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) == ERROR) {
        printf("Failed to spawn SRIO receive task\n");
    }

    return OK;
}
